#ifndef ROSCONNECTOR_H
#define ROSCONNECTOR_H

#include "ros/ros.h"
#include "sensor_msgs/Joy.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
#include <QThread>
#include <vector>
#include <Eigen/Dense>
#include "dynamixel_sdk_examples/BulkSetItem_Port3.h"
#include "dynamixel_sdk_examples/BulkSetItem_Port4.h"
#include "dynamixel_sdk_examples/BulkGetItem_five.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port3.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port4.h"
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int8.h>
#include"robotKinematics.hpp"
#include "epos_msgs/MotorStatesSRV.h"
#include <iostream>
#include <fstream>
#include <time.h>
#include <bits/stdc++.h>
#include "geometry_msgs/PoseStamped.h"
#include <string>
#include <widget.h>
#include <opencv2/opencv.hpp>
#include "apriltag_ros/AprilTagDetection.h"
#include "apriltag_ros/AprilTagDetectionArray.h"
#include <cv_bridge/cv_bridge.h>

class RosConnector
{
private:
    std::string init_node_name;
    ros::Subscriber m_Sub;
    ros::Subscriber simulation_joints_Sub;
    ros::Subscriber maxon_Get;
    ros::Subscriber optitrack_get;
    ros::Subscriber apriltag_corner_point;
    ros::Subscriber apriltag_tag_detections;
    ros::Publisher m_Pub_Port4;
    ros::Publisher m_Pub_Port3;
    ros::Publisher m_Pub_M;
    // ros::Publisher Pub_total_position;
    ros::Publisher simulation_image_Pub;
    ros::Publisher simulation_info_Pub;

    ros::ServiceClient bulk_Client_Port4;
    ros::ServiceClient bulk_Client_Port3;
    ros::ServiceClient maxon_Client;
    ros::NodeHandle m_nh;
    int rate_f;
    int lift_start;
    int lift_offset;
    
    dynamixel_sdk_examples::BulkSetItem_Port4 bulk_msg_port4;
    dynamixel_sdk_examples::BulkSetItem_Port3 bulk_msg_port3;
    epos_msgs::MotorStatesSRV maxon_srv;
    std_msgs::Float32MultiArray maxon_msg;
    std_msgs::Float32MultiArray total_msg;
    dynamixel_sdk_examples::BulkGetItem_Port4 bulk_Port4;
    dynamixel_sdk_examples::BulkGetItem_Port3 bulk_Port3;

    sensor_msgs::ImagePtr image_to_april;
    sensor_msgs::CameraInfo info_to_april;
    sensor_msgs::Image image_to_display;
    
    std::vector<double> *traj_motors;//轨迹规划电机角度

    QVector <QString> number_space = {"0","1","2","3","4","5","6","7","8","9","10"};
    QVector <QString> joints_namespace = {" ","刚性","柔性弯曲","柔性自转","偏摆","俯仰","自转","升降"};
    QVector <QString> adjust_up = {" "," 前","后/下"," 左"," 左偏"," 俯"," 逆"," 上/后"};
    QVector <QString> adjust_down = {" "," 后","前/上"," 右"," 右偏"," 仰"," 顺"," 下/前"};

    Widget* w;

    cv::Mat opencv_usbcam;
    std_msgs::Header header;

public:
    kinematics kine;
    bool manualPattern;
    int i_operate,i_screw,i_coverPlate;//进程标识符
    int operate_load = -1;
    int button_check_continue,button_check_Yes_No,button_check_left,button_check_right,button_check_cot,button_check_long_push;
    bool button_3_check;
    bool off_touch;
    bool screw_catch_button;
    int chg_target = 0;
    int chg_for_control = 0;
    double m_thx_chg;
    double m_thy_chg;
    double m_thz_chg;
    double m_x_chg;
    double m_y_chg;
    double m_z_chg;
    int startpoint_check = 0;
    int limited_joint = 0;
    int limited_joint_outsend = 0;
    int limited_warning_down = 0;
    int limited_warning_up=0;

    double m_thx, m_thy, m_thz, m_x, m_y, m_z;
    
    vector<vector<vector<vector<double>>>> cartesian_position;
    vector<vector<vector<vector<double>>>> cartesian_orientation;
  

    uint32_t image_height;//仿真图像参数
    uint32_t image_width;
    string image_encoding;
    uint8_t image_is_bigendian;
    uint32_t image_step;
    std::vector<uint8_t, std::allocator<uint8_t>> image_data;
    uint8_t image_uint8[921600]; //512*512*3 根据相机像素尺寸进行修改；
    std::vector<double> joint_simulation_position;
    std::vector<double> joint_simulation_position_rec;
    std::vector<std::vector<double>> motor_limit_value;
    std::vector<double> motors;
    std::vector<double> Motors_continuous_check;
    std::vector<double> motors_load;
    std::vector<double> m_param;
    std::vector<double> m_param_record;
    // cv::Mat CGet;
    std::vector<double> apriltag_corpoint = {0,0,0,0,0,0,0,0}; 
    std::vector<double> apriltag_position_posture = {0,0,0,0,0,0,0}; 

    QImage desImgGray_manual;

    RosConnector(ros::NodeHandle &nh, Widget &w);

    void call_back(const sensor_msgs::Joy &msg);
    void apriltag_call_back(const std_msgs::Float32MultiArray &msg);
    void apriltag_detections_call_back(const apriltag_ros::AprilTagDetectionArray &msg);
    void simulation_image_publish();
    void simulation_joints_call_back(const std_msgs::Float32MultiArray &msg);
    void get_cartesian(const sensor_msgs::Joy &msg);
    void pub_all_robot(/*const */vector<double> &motor_values);
    // void pub_total_position(const vector<double> &msg, int work_target);
    void check_direct();
    void position_mode();
    bool check_limit(const vector<double> &motors_check);
    void button_fit_check(const sensor_msgs::Joy &msg);

    void adjustmotor(const vector<double>& motor_values);
    vector<double> record_maxon_pos = {0,0,0,0};
    vector<double> maxon_reset = { 0,0,0,0 };

    vector<double> present_position_get();
    vector<double> present_position;

    void set_manual_start(const vector<double> &msg);

    Widget* ros_widget;

    vector<vector<vector<int>>> Index_library;
    vector<vector<int>> apriltags_search_library;

    DIRECT getmsg();
    bool init();
    bool init_finished;

    ~RosConnector();
};

#endif // ROSCONNECTOR_H
